#include <ros/ros.h>
#include "kinematic.h"
#include <ubt_core_msgs/JointCommand.h>
#include "webots_api/SceneSelection.h"

int main(int argc, char **argv) {
    Matrix_Init();
    ros::init(argc, argv, "SwitchLight_node");
    ros::NodeHandle nh;

    ros::ServiceClient client = nh.serviceClient<webots_api::SceneSelection>("/walker/sence");
    
    webots_api::SceneSelection scene_selection;
    scene_selection.request.scene_name = "SwitchLight";
    scene_selection.request.nav = false;
    scene_selection.request.vision = false;
    
    std::cout << "Wait for SceneSelect Service" << std::endl;
    while(!client.call(scene_selection))
    {
        if(!ros::ok())
        {
            return 0;
        }
    }
    ROS_INFO("Scene select success!");

    ros::Publisher left_arm_pub = nh.advertise<ubt_core_msgs::JointCommand>("/walker/leftLimb/controller", 10);
    
    ubt_core_msgs::JointCommand left_arm_data;
    left_arm_data.command.resize(7);
    left_arm_data.mode = 5;

    Eigen::VectorXd joints(7);
    joints << 0,0,0,0,0,0,0;
    Eigen::Vector3d position, orientation;

    ros::Rate loop_rate(1000);
    double time_count = 0.0;
    while(ros::ok())
    {
        time_count += 0.002;
        if(time_count <= 0.5)
        {
            orientation << 0, 0, 0;
            position << 350, 200 * sin(M_PI*time_count), 400 - 50 * sin(M_PI*time_count);
            if(leftArmInverse(position, orientation, joints, false) == true)
            {
                left_arm_data.command[0] = joints(0);
                left_arm_data.command[1] = joints(1);
                left_arm_data.command[2] = joints(2);
                left_arm_data.command[3] = joints(3);
                left_arm_data.command[4] = 0;
                left_arm_data.command[5] = 0;
                left_arm_data.command[6] = 0;
                left_arm_pub.publish(left_arm_data);
            }
        }
        else if(time_count <= 1.5)
        {
            position << 350 + 60 * sin(M_PI/2*time_count - M_PI/4), 200 + 105 * sin(M_PI/2*time_count - M_PI/4), 350 - 500 * sin(M_PI/2*time_count - M_PI/4);
            if(leftArmInverse(position, orientation, joints, false) == true)
            {
                left_arm_data.command[0] = joints(0);
                left_arm_data.command[1] = joints(1);
                left_arm_data.command[2] = joints(2);
                left_arm_data.command[3] = joints(3);
                left_arm_data.command[4] = 1.17 * sin(M_PI/2*time_count - M_PI/4);
                left_arm_data.command[5] = 0;
                left_arm_data.command[6] = 0;
                left_arm_pub.publish(left_arm_data);
            }
        }
        else if(time_count <= 7.5)
        {
            // 停顿3秒
        }
        else if(time_count <= 8)
        {
            // 关灯
            position << 410, 305, -130;
            if(leftArmInverse(position, orientation, joints, false) == true)
            {
                left_arm_data.command[0] = joints(0);
                left_arm_data.command[1] = joints(1);
                left_arm_data.command[2] = joints(2);
                left_arm_data.command[3] = joints(3);
                left_arm_data.command[4] = 1.17;
                left_arm_data.command[5] = 0;
                left_arm_data.command[6] = 0;
                left_arm_pub.publish(left_arm_data);
            }
        }
        
        ros::spinOnce();
        loop_rate.sleep();
    }
}